#include <iostream>
#include <ilcplex/ilocplex.h>
#include <fstream>

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
#include "HGJPathSmooth/pathSmoother.h"

using namespace std;

/**
 *  POINTS类型在添加的每个点处放置一个点。
 *  LINE_STRIP类型将每个点用作一组连接的线中的顶点，其中点0连接到点1，点1连接到点2，点2连接到点3等。
 *  LINE_LIST类型在每对点（即点0到1、2到3等）中创建未连接的线。
 */

int main( int argc, char** argv )
{
    ros::init(argc, argv, "points_and_lines");
    ros::NodeHandle n;

    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("/pathpoints_visualization_marker", 10);

    ros::Rate r(20);

    HGJ::pathPlanner planner;
    HGJ::WayPoints wayPoints;

    while (ros::ok()){
        
        ros::spinOnce();

        wayPoints = {{0,0,0},
                    {0.3,0,0},
                    {0.5,-0.6,0},
                    {-0.5,-1,0},
                    {-0.4,0,0},
                    {-0.6,0,0}};

        auto a = planner.genCurv(wayPoints, 0.01, 0.08);

        /// set the style of point and line in Rviz
        visualization_msgs::Marker markers, line_strip, line_list;
        markers.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "my_frame";
        markers.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
        markers.ns = line_strip.ns = line_list.ns = "points_and_lines";
        markers.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
        markers.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;

        markers.id = 0;
        line_strip.id = 1;
        line_list.id = 2;

        markers.type = visualization_msgs::Marker::POINTS;
        line_strip.type = visualization_msgs::Marker::LINE_STRIP;
        line_list.type = visualization_msgs::Marker::LINE_LIST;

        // POINTS markers use x and y scale for width/height respectively
        markers.scale.x = 0.03;
        markers.scale.y = 0.03;

        // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
        line_strip.scale.x = 0.01;
        line_list.scale.x = 0.01;

        // Points are green
        markers.color.g = 1.0f;
        markers.color.a = 1.0;

        // Line strip is blue
        line_strip.color.b = 1.0;
        line_strip.color.a = 1.0;

        // Line list is red
        line_list.color.r = 1.0;
        line_list.color.a = 1.0;

        // Create the vertices for the points and lines
        for (const auto & point: a) {
            geometry_msgs::Point p;
            p.x = point.getX();
            p.y = point.getY();
            p.z = point.getZ();
            line_strip.points.push_back(p);
        }

        for (const auto & markPlot: wayPoints) {

            geometry_msgs::Point pp;
            pp.x = markPlot.getX();
            pp.y = markPlot.getY();
            pp.z = markPlot.getZ();

            markers.points.push_back(pp);

        }

        marker_pub.publish(markers);
        marker_pub.publish(line_strip);

        r.sleep();

        ROS_INFO("----------FINISH THIS LOOP----------");
    }
}